<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN" "http://www.w3.org/TR/html4/loose.dtd">
<html lang="en">
<head>
<title>Source code</title>
<link rel="stylesheet" type="text/css" href="../../../../stylesheet.css" title="Style">
</head>
<body>
<div class="sourceContainer">
<pre><span class="sourceLineNo">001</span>package imagingbook.calibration.zhang;<a name="line.1"></a>
<span class="sourceLineNo">002</span><a name="line.2"></a>
<span class="sourceLineNo">003</span>import java.awt.geom.Point2D;<a name="line.3"></a>
<span class="sourceLineNo">004</span><a name="line.4"></a>
<span class="sourceLineNo">005</span>import org.apache.commons.math3.linear.ArrayRealVector;<a name="line.5"></a>
<span class="sourceLineNo">006</span>import org.apache.commons.math3.linear.DecompositionSolver;<a name="line.6"></a>
<span class="sourceLineNo">007</span>import org.apache.commons.math3.linear.MatrixUtils;<a name="line.7"></a>
<span class="sourceLineNo">008</span>import org.apache.commons.math3.linear.RealMatrix;<a name="line.8"></a>
<span class="sourceLineNo">009</span>import org.apache.commons.math3.linear.RealVector;<a name="line.9"></a>
<span class="sourceLineNo">010</span>import org.apache.commons.math3.linear.SingularValueDecomposition;<a name="line.10"></a>
<span class="sourceLineNo">011</span><a name="line.11"></a>
<span class="sourceLineNo">012</span><a name="line.12"></a>
<span class="sourceLineNo">013</span>/**<a name="line.13"></a>
<span class="sourceLineNo">014</span> * This class defines methods for estimating the radial lens distortion parameters<a name="line.14"></a>
<span class="sourceLineNo">015</span> * <a name="line.15"></a>
<span class="sourceLineNo">016</span> * @author W. Burger<a name="line.16"></a>
<span class="sourceLineNo">017</span> *<a name="line.17"></a>
<span class="sourceLineNo">018</span> */<a name="line.18"></a>
<span class="sourceLineNo">019</span>public class RadialDistortionEstimator {<a name="line.19"></a>
<span class="sourceLineNo">020</span>        <a name="line.20"></a>
<span class="sourceLineNo">021</span>        /**<a name="line.21"></a>
<span class="sourceLineNo">022</span>         * Estimates the lens distortion from multiple views, starting from an initial (linear) camera model.<a name="line.22"></a>
<span class="sourceLineNo">023</span>         * @param cam the initial (linear) camera model<a name="line.23"></a>
<span class="sourceLineNo">024</span>         * @param views a sequence of extrinsic view transformations<a name="line.24"></a>
<span class="sourceLineNo">025</span>         * @param modelPts the set of 2D model points (on the planar calibration target)<a name="line.25"></a>
<span class="sourceLineNo">026</span>         * @param obsPts a sequence of 2D image point sets, one set for each view<a name="line.26"></a>
<span class="sourceLineNo">027</span>         * @return a vector of lens distortion coefficients<a name="line.27"></a>
<span class="sourceLineNo">028</span>         */<a name="line.28"></a>
<span class="sourceLineNo">029</span>        protected double[] estimateLensDistortion(Camera cam, ViewTransform[] views, Point2D[] modelPts, Point2D[][] obsPts) {<a name="line.29"></a>
<span class="sourceLineNo">030</span>                final int M = views.length;<a name="line.30"></a>
<span class="sourceLineNo">031</span>                final int N = modelPts.length;<a name="line.31"></a>
<span class="sourceLineNo">032</span><a name="line.32"></a>
<span class="sourceLineNo">033</span>                final double uc = cam.getUc();<a name="line.33"></a>
<span class="sourceLineNo">034</span>                final double vc = cam.getVc();<a name="line.34"></a>
<span class="sourceLineNo">035</span><a name="line.35"></a>
<span class="sourceLineNo">036</span>                RealMatrix D = MatrixUtils.createRealMatrix(2 * M * N, 2);<a name="line.36"></a>
<span class="sourceLineNo">037</span>                RealVector d = new ArrayRealVector(2 * M * N);<a name="line.37"></a>
<span class="sourceLineNo">038</span><a name="line.38"></a>
<span class="sourceLineNo">039</span>                int l = 0;<a name="line.39"></a>
<span class="sourceLineNo">040</span>                for (int i = 0; i &lt; M; i++) {<a name="line.40"></a>
<span class="sourceLineNo">041</span>                        Point2D[] obs = obsPts[i];<a name="line.41"></a>
<span class="sourceLineNo">042</span><a name="line.42"></a>
<span class="sourceLineNo">043</span>                        for (int j = 0; j &lt; N; j++) {<a name="line.43"></a>
<span class="sourceLineNo">044</span>                                // determine the radius in the ideal image plane<a name="line.44"></a>
<span class="sourceLineNo">045</span>                                double[] xy = cam.projectNormalized(views[i], modelPts[j]);<a name="line.45"></a>
<span class="sourceLineNo">046</span>                                double x = xy[0];<a name="line.46"></a>
<span class="sourceLineNo">047</span>                                double y = xy[1];<a name="line.47"></a>
<span class="sourceLineNo">048</span>                                double r2 = x * x + y * y;<a name="line.48"></a>
<span class="sourceLineNo">049</span>                                double r4 = r2 * r2;<a name="line.49"></a>
<span class="sourceLineNo">050</span>                                <a name="line.50"></a>
<span class="sourceLineNo">051</span>                                // project model point to image<a name="line.51"></a>
<span class="sourceLineNo">052</span>                                double[] uv = cam.project(views[i], modelPts[j]);<a name="line.52"></a>
<span class="sourceLineNo">053</span>                                double u = uv[0];<a name="line.53"></a>
<span class="sourceLineNo">054</span>                                double v = uv[1];<a name="line.54"></a>
<span class="sourceLineNo">055</span>                                double du = u - uc;     // distance to estim. projection center<a name="line.55"></a>
<span class="sourceLineNo">056</span>                                double dv = v - vc;<a name="line.56"></a>
<span class="sourceLineNo">057</span>                                <a name="line.57"></a>
<span class="sourceLineNo">058</span>                                D.setEntry(l * 2 + 0, 0, du * r2);<a name="line.58"></a>
<span class="sourceLineNo">059</span>                                D.setEntry(l * 2 + 0, 1, du * r4);<a name="line.59"></a>
<span class="sourceLineNo">060</span>                                D.setEntry(l * 2 + 1, 0, dv * r2);<a name="line.60"></a>
<span class="sourceLineNo">061</span>                                D.setEntry(l * 2 + 1, 1, dv * r4);<a name="line.61"></a>
<span class="sourceLineNo">062</span>                                <a name="line.62"></a>
<span class="sourceLineNo">063</span>                                // observed image point<a name="line.63"></a>
<span class="sourceLineNo">064</span>                                Point2D UV = obs[j];<a name="line.64"></a>
<span class="sourceLineNo">065</span>                                double U = UV.getX();<a name="line.65"></a>
<span class="sourceLineNo">066</span>                                double V = UV.getY();<a name="line.66"></a>
<span class="sourceLineNo">067</span>                                <a name="line.67"></a>
<span class="sourceLineNo">068</span>                                d.setEntry(l * 2 + 0, U - u);<a name="line.68"></a>
<span class="sourceLineNo">069</span>                                d.setEntry(l * 2 + 1, V - v);<a name="line.69"></a>
<span class="sourceLineNo">070</span>                                l++;<a name="line.70"></a>
<span class="sourceLineNo">071</span>                        }<a name="line.71"></a>
<span class="sourceLineNo">072</span>                }<a name="line.72"></a>
<span class="sourceLineNo">073</span>                <a name="line.73"></a>
<span class="sourceLineNo">074</span>                DecompositionSolver solver = new SingularValueDecomposition(D).getSolver();<a name="line.74"></a>
<span class="sourceLineNo">075</span>                RealVector k = solver.solve(d);<a name="line.75"></a>
<span class="sourceLineNo">076</span>                <a name="line.76"></a>
<span class="sourceLineNo">077</span>//              double err1 = D.operate(new ArrayRealVector(new double[] {0,0})).subtract(d).getNorm();<a name="line.77"></a>
<span class="sourceLineNo">078</span>//              double err2 = D.operate(k).subtract(d).getNorm();<a name="line.78"></a>
<span class="sourceLineNo">079</span>//              System.out.format("err1=%.2f, err2=%.2f \n", err1, err2);<a name="line.79"></a>
<span class="sourceLineNo">080</span>                <a name="line.80"></a>
<span class="sourceLineNo">081</span>                return k.toArray();<a name="line.81"></a>
<span class="sourceLineNo">082</span>        }<a name="line.82"></a>
<span class="sourceLineNo">083</span><a name="line.83"></a>
<span class="sourceLineNo">084</span>}<a name="line.84"></a>




























































</pre>
</div>
</body>
</html>
